using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

namespace Autobot
{
    // All motors and sensor data
    // are identical for this project so
    // we can control them all with the following
    // parameters
    public class SensorParams
    {
        public const float MAX = 1f,
                           MIN = 0f,
                           RANGE = 0.8f,
                           NULL = -1f;
    }
    public enum DriveMotorParams : int
    {
        MAX = 102,
        MIN = 93,
        STOP = 100,
        NULL = -1
    }

    public class SteeringMotorParams
    {
        public const int MAX = 107, MIN = 77, STOP = 92, NULL = -1;
        public const int MAX_LEFT = MIN, MAX_RIGHT = MAX;
    }
    public enum SensorID : int
    {
        LEFT = 0,
        RIGHT,
        MIDDLE,
        NUM_SENSORS
    }
    public enum MotorID : int
    {
        DRIVE = 0,
        STEERING,
        NUM_MOTORS
    }

    // This will hold public info on the robot's data
    class RobotData
    {
        // Car's real position and direction vector
        // Direction vector must be a unit vector with magnitude of 1f
        public Vector2 realPosition;
        public Vector2 direction;

        //Car sensor and motor readings
        public float[] sensors;
        public int[] motors;

        //Physical movement properties of the car
        //Velocity in m/s  and acceleration in m/s2
        public float velocity;
        public float acceleration;

        public RobotData()
        {
            // Initial position to 0,0 facing right, stopped
            realPosition = new Vector2(0.0f, 0.0f);
            direction = new Vector2(1.0f, 0.0f);
            velocity = 0.0f;
            acceleration = 0.0f;

            //Init the sensor readings
            sensors = new float[(int)SensorID.NUM_SENSORS];
            for (int i = 0; i < (int)SensorID.NUM_SENSORS; i++)
            {
                //The sensor specs will need to be played with
                sensors[i] = (float)SensorParams.MAX;
            }
            //Init the motor readings
            motors = new int[(int)MotorID.NUM_MOTORS];
            
            motors[(int)MotorID.DRIVE] = (int)DriveMotorParams.STOP;
            motors[(int)MotorID.STEERING] = (int)SteeringMotorParams.STOP;
            
        }
        // We might need this memberwise copy constructor and assignment operator
        public RobotData(RobotData Copy)
        {
            // Initial position to 0,0 facing right, stopped
            this.realPosition = new Vector2(Copy.realPosition.X,Copy.realPosition.Y);
            this.direction = new Vector2(Copy.direction.X,Copy.direction.Y);
            this.velocity = Copy.velocity;
            this.acceleration = Copy.acceleration;

            //Init the sensor readings
            sensors = new float[(int)SensorID.NUM_SENSORS];
            for (int i = 0; i < (int)SensorID.NUM_SENSORS; i++)
            {
                //The sensor specs will need to be played with
                this.sensors[i] = Copy.sensors[i];
            }
            //Init the motor readings
            motors = new int[(int)MotorID.NUM_MOTORS];
            for (int i = 0; i < (int)MotorID.NUM_MOTORS; i++)
            {
                //From what Devin says about the motors, max = 256, min = 0, ref = 128
                // and I will guess that the motors can change settings very fast so it would
                // take it no more than a second to change the motor's setting.
                this.motors[i] = Copy.motors[i];
            }
        }
        public RobotData GetCopy()
        {
            return (RobotData)this.MemberwiseClone();
        }
    }
    class Robot
    {
        // This is where all the REAL data is stored
        private RobotData Data;

        //// The angle that the robot is currently facing
        //public double angle;
        //// The current driving angle step of the robot
        //private int drivingAngleStep;
        //// The current velocity step of the robot
        //private int velocityStep;

        //Graphics drawing stuff
        private SpriteBatch carBatch;
        private Vector2 spriteOrigin;
        private Texture2D sprite;
        private float angle;

        // This is the car's position in pixels on the window
        public Vector2 position;

        // Init a robot
        public Robot(Vector2 initPosition, double initAngle)
        {
            // Set the center position of the car
            position = initPosition;
            //angle = initAngle;

            // Initialize all the robot data
            Data = new RobotData();
        }

        // Be able to get all of the robot's current data without passing
        // the whole robot class. Be careful with which one you use. Changes
        // to the reference will affect the copy in Robot class.
        public RobotData GetDataReference()
        {
            return Data;
        }
        // Get the angle the robot is driving at
        public float GetAngle()
        {
            return angle;
        }
        public RobotData GetDataCopy()
        {
            return Data.GetCopy();
        }
        public Vector2 GetDirection()
        {
            return Data.direction;
        }
        public Vector2 GetRealPosition()
        {
            return Data.realPosition;
        }
        public Vector2 GetVirtualPosition()
        {
            return position;
        }
        public float GetVelocity()
        {
            return Data.velocity;
        }
        public float GetAcceleration()
        {
            return Data.acceleration;
        }
        public float GetSensorReading(int WhichSensor)
        {
            if (WhichSensor >= 0 && WhichSensor < (int)SensorID.NUM_SENSORS)
                return Data.sensors[WhichSensor];
            else
                return 0.0f;
        }
        public int GetMotorSetting(int WhichMotor)
        {
            if (WhichMotor >= 0 && WhichMotor < (int)MotorID.NUM_MOTORS)
                return Data.motors[WhichMotor];
            else
                return 0;
        }
        public void LoadContent(ContentManager Content, GraphicsDevice drawDevice)
        {
            //Initialize the car's sprite batch
            carBatch = new SpriteBatch(drawDevice);

            // Remember the graphics set to the robot
            sprite = Content.Load<Texture2D>("Robot");

            // Set the center of the sprite. This is to rotate around when drawing
            spriteOrigin = new Vector2(sprite.Width / 2, sprite.Height / 2);
            //spriteOrigin = new Vector2(0f, 0f);
        }

        public void Update(GameTime gameTime, Vector2 NewRealPosition, Vector2 NewVirtualPosition, Vector2 NewDirection, float NewVelocity, float[] NewSensorData, int[] NewMotorData)
        {
            SetRealPosition(NewRealPosition);
            SetVirtualPosition(NewVirtualPosition);
            SetDirection(NewDirection);
            UpdateVelocity(NewVelocity);

            if (NewSensorData != null)
                for (int i = 0; i < (int)SensorID.NUM_SENSORS; i++)
                    UpdateSensor(i, NewSensorData[i]);

            if (NewMotorData != null)
                for (int i = 0; i < (int)MotorID.NUM_MOTORS; i++)
                    UpdateMotor(i, NewMotorData[i]);
        }
        public void UpdateMotor(int WhichMotor, int NewSetting)
        {
            if(WhichMotor == (int)MotorID.DRIVE)
            {
                if (NewSetting >= (int)DriveMotorParams.MIN && NewSetting <= (int)DriveMotorParams.MAX)
                {
                    Data.motors[WhichMotor] = NewSetting;
                }
                else if (NewSetting < (int)DriveMotorParams.MIN)
                {
                    Data.motors[WhichMotor] = (int)DriveMotorParams.MIN;
                }
                else if (NewSetting > (int)DriveMotorParams.MAX)
                {
                    Data.motors[WhichMotor] = (int)DriveMotorParams.MAX;
                }
            }
            else if (WhichMotor == (int)MotorID.STEERING)
            {
                if (NewSetting >= (int)SteeringMotorParams.MIN && NewSetting <= (int)SteeringMotorParams.MAX)
                {
                    Data.motors[WhichMotor] = NewSetting;
                }
                else if (NewSetting < (int)SteeringMotorParams.MIN)
                {
                    Data.motors[WhichMotor] = (int)SteeringMotorParams.MIN;
                }
                else if (NewSetting > (int)SteeringMotorParams.MAX)
                {
                    Data.motors[WhichMotor] = (int)SteeringMotorParams.MAX;
                }
            }
        }
        public void SetVirtualPosition(Vector2 VirtualPos)
        {
            if(VirtualPos.X >= 0 && VirtualPos.Y >=0)
                position = new Vector2(VirtualPos.X,VirtualPos.Y);
        }
        public void SetRealPosition(Vector2 RealPos)
        {
            if(RealPos.X >= 0 && RealPos.Y >=0)
                Data.realPosition = new Vector2(RealPos.X,RealPos.Y);
        }
        public void UpdateSensor(int WhichSensor, float NewReading)
        {
            if (WhichSensor >= 0 && WhichSensor < (int)SensorID.NUM_SENSORS)
                if (NewReading >= (float)SensorParams.MIN && NewReading <= (float)SensorParams.MAX)
                    Data.sensors[WhichSensor] = NewReading;
        }
        public void SetDirection(Vector2 NewDirection)
        {
            // We want to make sure that the new direction is good and that it is normalized.
            if (NewDirection.X !=0 || NewDirection.Y !=0)
            {
                Data.direction = new Vector2(NewDirection.X,NewDirection.Y);
                if (Data.direction.Length() != 1.0f)
                    Data.direction.Normalize();

                // Remember to update the angle for drawing
                angle = (float)Math.Atan2((double)Data.direction.Y, (double)Data.direction.X);
            }
        }
        public void IncAngle(float Increment)
        {
            // Update the angle and the direction
            angle += Increment;
            Vector2 oldDirection = new Vector2(Data.direction.X, Data.direction.Y);
            Data.direction.X = (float)Math.Cos(Increment) * oldDirection.X - (float)Math.Sin(Increment) * oldDirection.Y;
            Data.direction.Y = (float)Math.Sin(Increment) * oldDirection.X + (float)Math.Cos(Increment) * oldDirection.Y;
        }
        public void UpdateVelocity(float NewMagnitude)
        {
            if(NewMagnitude >= 0)
                Data.velocity = NewMagnitude;
        }
        public void UpdateAcceleration(float NewMagnitude)
        {
            if(NewMagnitude >=0)
                Data.acceleration = NewMagnitude;
        }

        public void Draw()
        {
            // Calculate the drawing rectangle. position indicates the center of the car.
            // Now you might be thinking WTFBBQ is wrong with my draw rectangle? I though position was supposed to be in the middle!!! 
            // XNA has a M$ism where the 
            // spriteorigin parameter effs the rectangle up. The origin is where the sprite is rotated around, and scaled around,
            // simple enough. But when it comes to drawing, the rectangle is ALSO offset by this parameter. So we must magically
            // compensate so that our drawing rectangle is in the right spot. Boring.

            // Oh, and counter-intuitively, positive rotation is CW and negative rotation is CCW. GAH!.

            // Should be, but isn't: new Rectangle((int)(position.X - sprite.width/2), (int)(position.Y - sprite.height/2), sprite.Width, sprite.Height);
            Rectangle robotDrawRect = new Rectangle((int)(position.X), (int)(position.Y), sprite.Width, sprite.Height);

            carBatch.Begin(SpriteBlendMode.AlphaBlend);

            carBatch.Draw(sprite,               // The sprite to draw
                             robotDrawRect,     // The rectangle of where to draw the sprite
                             null,              // Rectangle which contains the sprite to draw, null is the entire sprite
                             Color.White,       // Apply a white tint to the sprite
                             (float)angle,      // The angle to rotate the sprite in radians
                             spriteOrigin,      // The origin point of the sprite which is rotated around
                             SpriteEffects.None,// Don't use any sprite effects
                             0.0f);             // :iiam:
            carBatch.End();
        }

        // Dispose of any attributes that need to be disposed
        public void Dispose()
        {
            // Dispose of the robot sprite if it hasn't been disposed of yet
            if(!sprite.IsDisposed)
            {
                sprite.Dispose();
            }

            //Dispose of the sprite batches
            if (!carBatch.IsDisposed)
                carBatch.Dispose();
        }

        //// ANGLE METHODS /////////////////////////////////////////////////
        //// Set the robot's current angle
        //public double SetAngle(double newAngle)
        //{
        //    // Make sure the angle is between zero and two-Pi
        //    // If the angle is below zero
        //    if (newAngle < 0)
        //    {
        //        // Convert the angle to a positive integer representation
        //        //uint intAngle = 360 * Math.Abs(newAngle) / Math.PI + 360;
        //        // Find the number of steps the new angle is below the current
        //        double temp = Math.Abs(newAngle);// +Math.PI;
        //        temp %= (2 * Math.PI);
        //    }
        //    // If the angle is greater than a full circle
        //    else if (newAngle > 2 * Math.PI)
        //    {
        //    }
        //    // If the angle doesn't need any changes
        //    else
        //        // Set the robot's angle to the given value
        //        angle = newAngle;
        //    // Return the robot's new angle
        //    return angle;
        //}

        //// Calculate the robot's current angle based on its state and return it as a unit direction
        //public Vector2 CalculateAngle()
        //{
        //    // Prepare the unit to return
        //    Vector2 returnAngle = Vector2.Zero;
        //    // Calculate the angle that the robot is driving at
        //    double drivingAngle = this.GetDrivingAngle();

        //    // Angle mod is a modifier to the current angle, default is no modifier
        //    double angleMod = 0;
        //    // If the car is in reverse
        //    if (velocityStep < 0)
        //    {
        //        // The  modifier is a half circle so the car moves backwards
        //        angleMod = Math.PI;
        //        // Because my math sucks and otherwise it doesn't go backwards properly
        //        angleMod += -2 * drivingAngle;
        //    }

        //    // Calculate the X/Y values of the robot's current angle
        //    returnAngle.X = (float)(Math.Cos(angle + drivingAngle + angleMod));
        //    returnAngle.Y = (float)(Math.Sin(angle + drivingAngle + angleMod));
        //    // Calculate the length of the return angle
        //    float unitLength = (float)(Math.Sqrt(Math.Pow(returnAngle.X, 2) + Math.Pow(returnAngle.Y, 2)));
        //    // Divide the X/Y values by the unit length to get the unit direction of the angle
        //    returnAngle.X /= unitLength;
        //    returnAngle.Y /= unitLength;
            
        //    // Return returnAngle as the angle the robot should be moving at
        //    return returnAngle;
        //}

        //// Turn the driving angle of the robot to the left, increasing its driving angle by a step
        //public double TurnLeft()
        //{
        //    drivingAngleStep = Math.Max(drivingAngleStep - 1, -DRIVING_ANGLE_STEPS);
        //    return this.GetDrivingAngle();
        //}
        

        //// Turn the driving angle of the robot to the right, decreasing its driving angle by a step
        //public double TurnRight()
        //{
        //    drivingAngleStep = Math.Min(drivingAngleStep + 1, DRIVING_ANGLE_STEPS);
        //    return this.GetDrivingAngle();
        //}

        //// SPEED AND VELOCITY METHODS ////////////////////////////////////
        //// Calculate the robot's speed and return it
        //public double GetSpeed()
        //{
        //    return velocityStep * (MAX_VELOCITY / VELOCITY_STEPS);
        //}

        //// Increase the robot's speed by one and return its new speed
        //public double IncSpeed()
        //{
        //    velocityStep = Math.Min(velocityStep + 1, VELOCITY_STEPS);
        //    return this.GetSpeed();
        //}

        //// Decrease the robot's speed by one and return its new speed
        //public double DecSpeed()
        //{
        //    velocityStep = Math.Max(velocityStep - 1, -VELOCITY_STEPS);
        //    return this.GetSpeed();
        //}

        // Return the robot's graphics
        public Texture2D GetSprite()
        {
            return sprite;
        }    
    }
}
